Simultaneous localization and mapping for a mobile robot

ABSTRACT

A method of simultaneous localization and mapping includes initializing a robot pose and a particle model of a particle filter. The particle model includes particles, each having an associated map, robot pose, and weight. The method includes receiving sparse sensor data from a sensor system of the robot, synchronizing the received sensor data with a change in robot pose, accumulating the synchronized sensor data over time, and determining a robot localization quality. When the accumulated sensor data exceeds a threshold accumulation and the robot localization quality is greater than a threshold localization quality, the method includes updating particles with accumulated synchronized sensor data. The method includes determining a weight for each updated particle of the particle model and setting a robot pose belief to the robot pose of the particle having the highest weight when a mean weight of the particles is greater than a threshold particle weight.

CROSS REFERENCE TO RELATED APPLICATIONS

This U.S. patent application is a continuation of, and claims priorityunder 35 U.S.C. §120 from, U.S. patent application Ser. No. 13/790,643,filed on Mar. 8, 2013, which claims priority under 35 U.S.C. §119(e) toU.S. Provisional Application 61/721,886, filed on Nov. 2, 2012. Thedisclosures of these prior applications are considered part of thedisclosure of this application and are hereby incorporated by referencein their entireties.

TECHNICAL FIELD

This disclosure relates to simultaneous localization and mapping (SLAM)for mobile robots.

BACKGROUND

A robot is generally an electro-mechanical machine guided by a computeror electronic programming. Mobile robots have the capability to movearound in their environment and are not fixed to one physical location.An example of a mobile robot that is in common use today is an automatedguided vehicle or automatic guided vehicle (AGV). An AGV is generally amobile robot that follows markers or wires in the floor, or uses avision system or lasers for navigation. Mobile robots can be found inindustry, military and security environments. They also appear asconsumer products, for entertainment or to perform certain tasks likevacuum cleaning and home assistance.

SUMMARY

One aspect of the disclosure provides a method of simultaneouslocalization and mapping executed on a controller of an autonomousmobile robot. The method includes initializing a robot pose andinitializing a particle model of a particle filter executing on thecontroller. The particle model includes particles, each having anassociated map, robot pose, and weight. The method also includesreceiving sparse sensor data from a sensor system of the robot,synchronizing the received sensor data with a change in robot pose,accumulating the synchronized sensor data over time in non-transitorymemory, and determining a localization quality of the robot. When theaccumulated sensor data exceeds a threshold accumulation and the robotlocalization quality is greater than a threshold localization quality,the method includes updating particles of the particle model withaccumulated synchronized sensor data. The method includes determining aweight for each updated particle of the particle model and setting arobot pose belief to the robot pose of the particle having the highestweight when a mean weight of the particles is greater than a thresholdparticle weight.

A computer program product encoded on a non-transitory computer readablestorage medium may include instructions that when executed by a dataprocessing apparatus cause the data processing apparatus to perform themethod of simultaneous localization and mapping described above.

Implementations of the disclosure may include one or more of thefollowing features. In some implementations, the method includessampling the particle model for a particle population and resampling theparticle model for the particle population when the robot localizationquality is greater than the threshold localization quality. The methodmay include replacing a portion of the particle population with randomlyselected particles of the particle model when resampling fails to occurwithin a threshold sampling time period. Additionally or alternatively,the method may include replacing a portion of the particle populationwith particles selected from the particle model based on the accumulatedsynchronized sensor data when resampling fails to occur within athreshold sampling time period.

The method may include determining the change in robot pose usingodometry received from a drive system of the robot and inertialmeasurements from an inertial measurement unit of the robot. In someexamples, determining the robot localization quality includes computingan instantaneous localization, receiving the instantaneous localizationthrough a fast low-pass filter, receiving the instantaneous localizationthrough a slow low-pass filter, and receiving the instantaneouslocalization through a leaky integrator. The method may includecomparing a ratio of results of the two low-pass filters integrated bythe integrator. When a result of the integrator is above a thresholdvalue, the method may include determining the robot as well localized(e.g., by assigning a localization quality accordingly).

The sensor data may include range data (e.g., from one or more infraredrange finding sensors) and the synchronized sensor data include rangemeasurement and bearing pairs. The bearings may be determined fromreceived odometry.

In some implementations, the method includes updating each particle'spose, map, and weight. Updating the particle pose may include applying arobot motion model to the accumulated synchronized sensor data. Therobot motion model models movement of the robot based on odometry andinertial measurements. Updating the particle map may include executing aray trace of the accumulated synchronized sensor data and applying arange sensor model to the accumulated synchronized sensor data. Updatingthe particle weight may be based on a number of measured ranges of theaccumulated synchronized sensor data matching predicted ranges.

Executing the ray trace may include, for each range measurement andbearing pair, receiving the bearing and an existing particle map, andcomputing a predicted range. The method may further include receiving ameasured range, receiving the predicted range, and computing aprobability of occupancy of an object in each cell of the particle mapalong the ray trace. In some examples, applying the range sensor modelincludes only updating particle map cells corresponding to thesynchronized sensor data.

In some implementations, applying the range sensor model includescomputing a range error as an absolute difference between the measuredrange and the predicted range and computing the occupancy probabilitywithin each cell of the particle map along the ray trace based on therange error. For each particle map cell along the ray trace, the methodmay include computing a first occupancy probability assuming detectionof an expected object, computing a second occupancy probability assumingdetection of an expectedly close object, computing a third occupancyprobability assuming failure to detect of an expected close objectwithin a threshold distance of the robot, and computing a weightedaverage of the first, second, and third occupancy probabilities.

Another aspect of the disclosure provides an autonomous mobile robotthat includes a drive system configured to maneuver the robot over afloor surface and a sensor system having at least one infrared rangefinding sensor producing range data of a scene about the robot. Therobot also includes a controller in communication with the drive systemand the sensor system. The controller executes a simultaneouslocalization and mapping method that includes initializing a robot poseand initializing a particle model of a particle filter executing on thecontroller. The particle model includes particles, each having anassociated map, robot pose, and weight. The method also includesreceiving sparse sensor data from a sensor system of the robot,synchronizing the received sensor data with a change in robot pose,accumulating the synchronized sensor data over time in non-transitorymemory, and determining a localization quality of the robot. When theaccumulated sensor data exceeds a threshold accumulation and the robotlocalization quality is greater than a threshold localization quality,the method includes updating particles of the particle model withaccumulated synchronized sensor data. The method includes determining aweight for each updated particle of the particle model and setting arobot pose belief to the robot pose of the particle having the highestweight when a mean weight of the particles is greater than a thresholdparticle weight.

In some implementations, the method includes sampling the particle modelfor a particle population and resampling the particle model for theparticle population when the robot localization quality is greater thanthe threshold localization quality. The method may include replacing aportion of the particle population with randomly selected particles ofthe particle model when resampling fails to occur within a thresholdsampling time period. Additionally or alternatively, the method mayinclude replacing a portion of the particle population with particlesselected from the particle model based on the accumulated synchronizedsensor data when resampling fails to occur within a threshold samplingtime period.

The method may include determining the change in robot pose usingodometry received from a drive system of the robot and inertialmeasurements from an inertial measurement unit of the robot. In someexamples, determining the robot localization quality includes computingan instantaneous localization, receiving the instantaneous localizationthrough a fast low-pass filter, receiving the instantaneous localizationthrough a slow low-pass filter, and receiving the instantaneouslocalization through a leaky integrator. The method may includecomparing a ratio of results of the two low-pass filters integrated bythe integrator. When a result of the integrator is above a thresholdvalue, the method may include determining the robot as well localized(e.g., by assigning a localization quality accordingly).

The sensor data may include range data (e.g., from one or more infraredrange finding sensors) and the synchronized sensor data include rangemeasurement and bearing pairs. The bearings may be determined fromreceived odometry.

In some implementations, the method includes updating each particle'spose, map, and weight. Updating the particle pose may include applying arobot motion model to the accumulated synchronized sensor data. Therobot motion model models movement of the robot based on odometry andinertial measurements. Updating the particle map may include executing aray trace of the accumulated synchronized sensor data and applying arange sensor model to the accumulated synchronized sensor data. Updatingthe particle weight may be based on a number of measured ranges of theaccumulated synchronized sensor data matching predicted ranges.

Executing the ray trace may include, for each range measurement andbearing pair, receiving the bearing and an existing particle map, andcomputing a predicted range. The method may further include receiving ameasured range, receiving the predicted range, and computing aprobability of occupancy of an object in each cell of the particle mapalong the ray trace. In some examples, applying the range sensor modelincludes only updating particle map cells corresponding to thesynchronized sensor data.

In some implementations, applying the range sensor model includescomputing a range error as an absolute difference between the measuredrange and the predicted range and computing the occupancy probabilitywithin each cell of the particle map along the ray trace based on therange error. For each particle map cell along the ray trace, the methodmay include computing a first occupancy probability assuming detectionof an expected object, computing a second occupancy probability assumingdetection of an expectedly close object, computing a third occupancyprobability assuming failure to detect of an expected close objectwithin a threshold distance of the robot, and computing a weightedaverage of the first, second, and third occupancy probabilities.

Another aspect of the disclosure provides a method executed on acontroller of an autonomous mobile robot. The method includesdetermining a localization quality of the robot and when thelocalization quality is below a threshold quality, accessing a goodparticle from a particle model of a particle filter executing on thecontroller. The particle model has particles, each having an associatedmap, robot pose, and weight. The good particle has weight greater than athreshold weight. The method includes generating new particles based onthe last known good particle and re-populating at least a portion of theparticle model with the new particles.

In some implementations, accessing a good particle includes accessing alast known good particle. The method may include ceasing map buildingwhen the localization quality is below the threshold quality.Determining the robot localization quality may include computing aninstantaneous localization, receiving the instantaneous localizationthrough a fast low-pass filter, receiving the instantaneous localizationthrough a slow low-pass filter, and receiving the instantaneouslocalization through a leaky integrator.

The method may include initializing a robot pose and initializing theparticle model of the particle filter executing on the controller. Themethod may also include receiving sparse sensor data from a sensorsystem of the robot, synchronizing the received sensor data with achange in robot pose, accumulating the synchronized sensor data overtime in non-transitory memory, and determining a localization quality ofthe robot. When the accumulated sensor data exceeds a thresholdaccumulation and the robot localization quality is greater than athreshold localization quality, the method may include updatingparticles of the particle model with accumulated synchronized sensordata. The method may include determining a weight for each updatedparticle of the particle model and setting a robot pose belief to therobot pose of the particle having the highest weight when a mean weightof the particles is greater than a threshold particle weight.

In some implementations, the method includes sampling the particle modelfor a particle population and resampling the particle model for theparticle population when the robot localization quality is greater thanthe threshold localization quality. The method may include replacing aportion of the particle population with randomly selected particles ofthe particle model when resampling fails to occur within a thresholdsampling time period. Additionally or alternatively, the method mayinclude replacing a portion of the particle population with particlesselected from the particle model based on the accumulated synchronizedsensor data when resampling fails to occur within a threshold samplingtime period.

The sensor data may include range data (e.g., from one or more infraredrange finding sensors) and the synchronized sensor data include rangemeasurement and bearing pairs. The bearings may be determined fromreceived odometry.

In some implementations, the method includes updating each particle'spose, map, and weight. Updating the particle pose may include applying arobot motion model to the accumulated synchronized sensor data. Therobot motion model models movement of the robot based on odometry andinertial measurements. Updating the particle map may include executing aray trace of the accumulated synchronized sensor data and applying arange sensor model to the accumulated synchronized sensor data. Updatingthe particle weight may be based on a number of measured ranges of theaccumulated synchronized sensor data matching predicted ranges.

The details of one or more implementations of the disclosure are setforth in the accompanying drawings and the description below. Otheraspects, features, and advantages will be apparent from the descriptionand drawings, and from the claims.

DESCRIPTION OF DRAWINGS

FIG. 1 is a front, top perspective view of an exemplary mobile robot.

FIG. 2 is a bottom, rear perspective view of the mobile robot shown inFIG. 1.

FIG. 3 is a partial exploded perspective view of the mobile robot shownin FIG. 1.

FIG. 4A is a schematic view of an exemplary mobile robot having fourinfrared range finding sensors.

FIGS. 4B and 4C are schematic views of an exemplary infrared rangefinding sensor detecting an object.

FIG. 5 is a schematic view of an exemplary controller for a mobilerobot.

FIG. 6 is a schematic view of an exemplary mobile robot having anavigation system.

FIG. 7 is a schematic view of an exemplary navigation system for mobilerobot.

FIG. 8A is schematic view of an exemplary robot scanning its environmentto obtain range data.

FIG. 8B is schematic view of an exemplary occupancy grid map.

FIG. 9 provides an exemplary arrangement of operations for a method ofsimultaneous localization and mapping (SLAM).

FIG. 10A provides an exemplary arrangement of operations for a method ofdetermining a robot localization quality.

FIG. 10B provides an exemplary arrangement of operations for a method ofdetermining a robot re-localization.

FIG. 11 provides an exemplary arrangement of operations for a method ofupdating a particle in a particle model.

FIG. 12 provides an exemplary arrangement of operations for a method ofapplying a range sensor model.

FIG. 13 provides an exemplary arrangement of operations for a method ofsimultaneous localization and mapping executed on a controller of amobile robot.

Like reference symbols in the various drawings indicate like elements.

DETAILED DESCRIPTION

An autonomous robot movably supported may execute simultaneouslocalization and mapping (SLAM) on a controller to navigate andnegotiate obstacles. For example, an autonomous cleaning robot mayexecute a SLAM routine navigate a cleaning area and to clean a surfacewhile traversing that area.

Referring to FIGS. 1-3, in some implementations, a robot 100 includes abody 110 supported by a drive system 120 that can maneuver the robot 100across the floor surface 10 based on a drive command having x, y, and θcomponents, for example, issued by a controller 150. The robot body 110has a forward portion 112 and a rearward portion 114. The drive system120 includes right and left driven wheel modules 120 a, 120 b that mayprovide odometry to the controller 150. The wheel modules 120 a, 120 bare substantially opposed along a transverse axis X defined by the body110 and include respective drive motors 122 a, 122 b driving respectivewheels 124 a, 124 b. The drive motors 122 a, 122 b may releasablyconnect to the body 110 (e.g., via fasteners or tool-less connections)with the drive motors 122 a, 122 b optionally positioned substantiallyover the respective wheels 124 a, 124 b. The wheel modules 120 a, 120 bcan be releasably attached to the chassis 110 and forced into engagementwith the cleaning surface 10 by respective springs. The robot 100 mayinclude a caster wheel 126 disposed to support a forward portion 112 ofthe robot body 110. The robot body 110 supports a power source 102(e.g., a battery) for powering any electrical components of the robot100.

The robot 100 can move across the cleaning surface 10 through variouscombinations of movements relative to three mutually perpendicular axesdefined by the body 110: a transverse axis X, a fore-aft axis Y, and acentral vertical axis Z. A forward drive direction along the fore-aftaxis Y is designated F (sometimes referred to hereinafter as “forward”),and an aft drive direction along the fore-aft axis Y is designated A(sometimes referred to hereinafter as “rearward”). The transverse axis Xextends between a right side R and a left side L of the robot 100substantially along an axis defined by center points of the wheelmodules 120 a, 120 b.

A forward portion 112 of the body 110 carries a bumper 130, whichdetects (e.g., via one or more sensors) one or more events in a drivepath of the robot 100, for example, as the wheel modules 120 a, 120 bpropel the robot 100 across the cleaning surface 10 during a cleaningroutine. The robot 100 may respond to events (e.g., obstacles, cliffs,walls) detected by the bumper 130 by controlling the wheel modules 120a, 120 b to maneuver the robot 100 in response to the event (e.g., awayfrom an obstacle). While some sensors are described herein as beingarranged on the bumper, these sensors can additionally or alternativelybe arranged at any of various different positions on the robot 100.

A user interface 140 disposed on a top portion of the body 110 receivesone or more user commands and/or displays a status of the robot 100. Theuser interface 140 is in communication with the robot controller 150carried by the robot 100 such that one or more commands received by theuser interface 140 can initiate execution of a cleaning routine by therobot 100.

The robot controller 150 (executing a control system) may executebehaviors 300 (FIG. 7) that cause the robot 100 to take an action, suchas maneuvering in a wall following manner, a floor scrubbing manner, orchanging its direction of travel when an obstacle is detected. The robotcontroller 150 can maneuver the robot 100 in any direction across thecleaning surface 10 by independently controlling the rotational speedand direction of each wheel module 120 a, 120 b. For example, the robotcontroller 150 can maneuver the robot 100 in the forward F, reverse(aft) A, right R, and left L directions. As the robot 100 movessubstantially along the fore-aft axis Y, the robot 100 can make repeatedalternating right and left turns such that the robot 100 rotates backand forth around the center vertical axis Z (hereinafter referred to asa wiggle motion). The wiggle motion can allow the robot 100 to operateas a scrubber during cleaning operation. Moreover, the wiggle motion canbe used by the robot controller 150 to detect robot stasis. Additionallyor alternatively, the robot controller 150 can maneuver the robot 100 torotate substantially in place such that the robot 100 can maneuver outof a corner or away from an obstacle, for example. The robot controller150 may direct the robot 100 over a substantially random (e.g.,pseudo-random) path while traversing the cleaning surface 10. The robotcontroller 150 can be responsive to one or more sensors (e.g., bump,proximity, wall, stasis, and cliff sensors) disposed about the robot100. The robot controller 150 can redirect the wheel modules 120 a, 120b in response to signals received from the sensors, causing the robot100 to avoid obstacles and clutter while treating the cleaning surface10. If the robot 100 becomes stuck or entangled during use, the robotcontroller 150 may direct the wheel modules 120 a, 120 b through aseries of escape behaviors so that the robot 100 can escape and resumenormal cleaning operations.

The robot 100 may include a cleaning system 160 for cleaning or treatingthe floor surface 10. The cleaning system 160 may include a dry cleaningsystem 160 a and/or a wet cleaning system 160 b. The dry cleaning system160 may include a driven roller brush 162 (e.g., with bristles and/orbeater flaps) extending parallel to the transverse axis X and rotatablysupported by the robot body 110 to contact the floor surface 10. Thedriven roller brush agitates debris off of the floor surface 10 andthrows or guides the agitated debris into a collection bin 163. The drycleaning system 160 may also include a side brush 164 having an axis ofrotation at an angle with respect to the floor surface 10 for movingdebris into a cleaning swath area of the cleaning system 160. The wetcleaning system 160 b may include a fluid applicator 166 that extendsalong the transverse axis X and dispenses cleaning liquid onto thesurface 10. The dry and/or wet cleaning systems 160 a, 160 b may includeone or more squeegee vacuums 168 (e.g., spaced apart compliant bladeshave a partial vacuum applied therebetween via an air pump) vacuumingthe cleaning surface 10.

To achieve reliable and robust autonomous movement, the robot 100 mayinclude a sensor system 500 having several different types of sensorswhich can be used in conjunction with one another to create a perceptionof the robot's environment sufficient to allow the robot 100 to makeintelligent decisions about actions to take in that environment. Thesensor system 500 may include one or more types of sensors supported bythe robot body 110, which may include obstacle detection obstacleavoidance (ODOA) sensors, communication sensors, navigation sensors,etc. For example, these sensors may include, but not limited to, rangefinding sensors, proximity sensors, contact sensors, a camera (e.g.,volumetric point cloud imaging, three-dimensional (3D) imaging or depthmap sensors, visible light camera and/or infrared camera), sonar, radar,LIDAR (Light Detection And Ranging, which can entail optical remotesensing that measures properties of scattered light to find range and/orother information of a distant target), LADAR (Laser Detection andRanging), etc. In some implementations, the sensor system 500 includesranging sonar sensors, proximity cliff detectors, contact sensors, alaser scanner, and/or an imaging sonar.

There are several challenges involved in placing sensors on a roboticplatform. First, the sensors need to be placed such that they havemaximum coverage of areas of interest around the robot 100. Second, thesensors may need to be placed in such a way that the robot 100 itselfcauses an absolute minimum of occlusion to the sensors; in essence, thesensors cannot be placed such that they are “blinded” by the robotitself. Third, the placement and mounting of the sensors should not beintrusive to the rest of the industrial design of the platform. In termsof aesthetics, it can be assumed that a robot with sensors mountedinconspicuously is more “attractive” than otherwise. In terms ofutility, sensors should be mounted in a manner so as not to interferewith normal robot operation (snagging on obstacles, etc.).

Referring to FIGS. 1 and 4A, in some implementations, the sensor system500 includes one or more range finding sensors 510 disposed on the robotbody 110 or bumper 130. In the example shown in FIG. 1, the sensorsystem 500 includes three range finders 510 a-c disposed on the bumper130 and arranged with a field of view 512 substantially normal to therobot body 110 (e.g., radially outward). One range finder 510 a may haveits field of view 512 centered along the forward drive direction F. Thefield of view 512 may have an angle β of between about 5° and about 45°.Moreover, one or more of the range finders 510 a-c may scan side-to-sideand/or up-and-down with respect to the forward drive direction F toincrease a lateral and vertical field of view 512 of the respectiverange finder 510. In the example shown, the sensor system 500 includesfirst, second, third, and fourth range finders 510 a-d disposed on therobot body 110 and arranged equilaterally with respect to each other(e.g., spaced every 90 degrees about the round robot body 110). The fourrange finders 510 a-d may be arranged as forward (i.e., directed alongthe forward drive direction F), right, left, and rearward (opposite offorward drive direction F) sensors. Other arrangements are possible aswell, such just one range finder 510 (e.g., with the use of odometry) ormany range finders disposed around the robot body 110. The range finders510 a-d can detect a distance to objects 12 in a scene 10 about therobot and in the field of view 512 of the range finders 510 a-d.

The infrared range finders 510 a-d offer a relatively low-cost, smallpackage solution compared to a rotating scanner that may requiremounting on top of the robot 100. A top mounted upwardly protrudingscanning sensor (to obtain the 360 degree rotating field of view) maybecome a problem for obstacle avoidance. For example, such a sensor cancause wedging problems when the robot 100 attempts to pass under anobstacle.

Referring to FIGS. 4B and 4C, the range finder 510 may be an infraredrange finding sensor that employs triangulation. The range finder 510includes an emitter 514 that emits a pulse of light (e.g., wavelengthrange of 850 nm +/−70 nm) and a detector 516 that detects a reflectionback (or no reflection at all). When the emitted light reflects off of asurface of an object 12, the reflected light returns at an angle αdependent on a distance D of the reflecting object 12. By detecting thereflected beam angle α, the sensor system 500 can determine the distanceD to the reflecting object 12. In some examples, the detector 516 has aprecision lens that transmits the received reflected light onto anenclosed linear CCD (charge coupled device) array based on thetriangulation angle α. The CCD array may determine the angle α andprovide a corresponding analog value. The range finder 510 may apply amodulated frequency to the emitted IR beam, which can make the rangedetecting immune to interference from ambient light. In other words, therange finder 510 may be capable of detecting a wall in full sunlight.

The bumper 130 may include one or more bump sensors 530 (e.g., contactsensor, switch, or infrared proximity sensor) for sensing contact with abumped object. In some examples, the bumper 130 includes right and leftbump sensors 530 a, 530 b for sensing a directionality of the bump withrespect to the forward drive direction (e.g., a bump vector).

Referring to FIGS. 5 and 6, in some implementations, reasoning orcontrol software, executable on the controller 150 (e.g., on a computingprocessor), uses a combination of algorithms executed using various datatypes generated by the sensor system 500. The reasoning softwareprocesses the data collected from the sensor system 500 and outputs datafor making navigational decisions on where the robot 100 can movewithout colliding with an obstacle, for example.

To operate autonomously, the robot 100 may use a navigation system 600to simultaneously localize and map its surroundings, using sensoryinputs from the sensor system 500. Simultaneous localization and mapping(SLAM) is a technique the robot 100 may use to build up a map 620 (e.g.,an occupancy map) within an unknown environment or scene 10 (withouta-priori knowledge), or to update the map 620 within a known environment(with a-priori knowledge from a given map), while at the same timekeeping track of its current location. The navigation system 600 allowsthe robot 100 to navigate a scene 10 without colliding into obstacles 12or falling down stairs. The navigation system 600 may be a behaviorbased system stored and/or executed on the robot controller 150.Moreover, the navigation system 600 may communicate with the drivesystem 120 and the sensor system 500 to receive odometry and sensordata, such as gyroscopic data from an inertial measurement unit 520and/or range data 515, to determine and issue drive commands to thedrive system 120.

Odometry is the use of sensor data to estimate change in position overtime (distance traveled). In some examples, an encoder is disposed onthe drive system 200 for measuring wheel revolutions, therefore adistance traveled by the robot 100. The controller 150 may use odometryin assessing a confidence level for a robot location. In someimplementations, the sensor system 500 includes an odometer and/or anangular rate sensor (e.g., gyroscope or the IMU 520) for sensing adistance traveled by the robot 100. A gyroscope is a device that can beused for measuring and/or maintaining orientation, based on theprinciples of conservation of angular momentum. The controller 150 mayuse odometry and/or gyro signals received from the odometer and/orangular rate sensor, respectively, to determine a location of the robot100 in a working floor area 5. In some examples, the controller 150 usesdead reckoning. Dead reckoning is the process of estimating a currentposition based upon a previously determined position, and advancing thatposition based upon known or estimated speeds over elapsed time, andcourse. By knowing a robot location in the working area 5 (e.g., viaodometry, gyroscope, etc.) as well as a sensed location of one or moreobjects 12 in the working area 5 (via the sensor system 500), thecontroller 150 can assess a relatively higher confidence level of alocation or movement of the robot 100 on an occupancy map 620 and in theworking area 5 (versus without the use of odometry or a gyroscope).

Referring to FIG. 5, in some implementations, the controller 150 (e.g.,a device having one or more computing processors in communication withmemory capable of storing instructions executable on the computingprocessor(s)) executes a control system 210, which includes a behaviorsystem 210 a and a control arbitration system 210 b in communicationwith each other. The control arbitration system 210 b allows robotapplications 220 to be dynamically added and removed from the controlsystem 210, and facilitates allowing applications 220 to each controlthe robot 100 without needing to know about any other applications 220.In other words, the control arbitration system 210 b provides a simpleprioritized control mechanism between applications 220 and resources 240of the robot 100.

The applications 220 can be stored in memory of or communicated to therobot 100, to run concurrently on (e.g., on a processor) andsimultaneously control the robot 100. The applications 220 may accessbehaviors 300 of the behavior system 210 a. The independently deployedapplications 220 are combined dynamically at runtime and to share robotresources 240 (e.g., drive system 120 and/or cleaning systems 160, 160a, 160 b). A low-level policy is implemented for dynamically sharing therobot resources 240 among the applications 220 at run-time. The policydetermines which application 220 has control of the robot resources 240as required by that application 220 (e.g. a priority hierarchy among theapplications 220). Applications 220 can start and stop dynamically andrun completely independently of each other. The control system 210 alsoallows for complex behaviors 300, 300 a-n which can be combined togetherto assist each other.

The control arbitration system 210 b includes one or more application(s)220 in communication with a control arbiter 260. The control arbitrationsystem 210 b may include components that provide an interface to thecontrol arbitration system 210 b for the applications 220. Suchcomponents may abstract and encapsulate away the complexities ofauthentication, distributed resource control arbiters, commandbuffering, coordinate the prioritization of the applications 220 and thelike. The control arbiter 260 receives commands from every application220 generates a single command based on the applications' priorities andpublishes it for its associated resources 240. The control arbiter 260receives state feedback from its associated resources 240 and may sendit back up to the applications 220. The robot resources 240 may be anetwork of functional modules (e.g., actuators, drive systems, andgroups thereof) with one or more hardware controllers. The commands ofthe control arbiter 260 are specific to the resource 240 to carry outspecific actions. A dynamics model 230 executable on the controller 150is configured to compute the center for gravity (CG), moments ofinertia, and cross products of inertial of various portions of the robot100 for the assessing a current robot state.

In some implementations, a behavior 300 is a plug-in component thatprovides a hierarchical, state-full evaluation function that couplessensory feedback from multiple sources, such as the sensor system 500,with a-priori limits and information into evaluation feedback on theallowable actions of the robot 100. Since the behaviors 300 arepluggable into the application 220 (e.g. residing inside or outside ofthe application 220), they can be removed and added without having tomodify the application 220 or any other part of the control system 210.Each behavior 300 is a standalone policy. To make behaviors 300 morepowerful, it is possible to attach the output of multiple behaviors 300together into the input of another so that you can have complexcombination functions. The behaviors 300 are intended to implementmanageable portions of the total cognizance of the robot 100.

In the example shown, the behavior system 210 a includes an obstacledetection/obstacle avoidance (ODOA) behavior 300 a for determiningresponsive robot actions based on obstacles perceived by the sensor(e.g., turn away; turn around; stop before the obstacle, etc.). Anotherbehavior 300 may include a wall following behavior 300 b for drivingadjacent to a detected wall (e.g., in a wiggle pattern of driving towardand away from the wall).

Referring to FIGS. 6 and 7, in some implementations, the navigationsystem 600 includes a simultaneous localization and mapping (SLAM)controller 610, executable on a computing processor (e.g., on the robotcontroller 150), that builds maps 620 (e.g., feature based maps,occupancy maps and/or ground plane maps) using sensor data received fromthe sensor system 500. In some examples, the SLAM controller 610 relieson range data 515 (points) received from the range finders 510 a-d,gyroscopic data 525 received from the inertial measurement unit 520,bump data 535 received from the bump sensor(s) 530, 503 a-b, and/orodometry 205 received from the drive system 200.

Maps 620 can be used to determine a location within a scene orenvironment 10 and to depict an environment for planning and navigation.The maps 620 support the assessment of actual location by recordinginformation obtained from a form of perception and comparing it to acurrent set of perceptions. The benefit of a map 620 in aiding theassessment of a location increases as the precision and quality of thecurrent perceptions decrease. Maps 620 generally represent the state atthe time that the map 620 is provided or produced. This is notnecessarily consistent with the state of the environment at the time themap 620 is used. Other localization techniques include monocular visualSLAM (MonoSLAM) and implementations using an extended Kalman filter(EKF) for MonoSLAM solutions.

FIG. 8A illustrates the robot 100 scanning its environment, a scene 10,to obtain range data 515. In this case, a range scan 517 detects a wall7 intersecting the floor 5 in the scene 10, demarcating a trackablefeature 14. FIG. 8B illustrates an exemplary occupancy grid map 620,which may be a 2D-XY grid 622 having cells 624 along an X direction anda Y direction. Each cell 624 of the occupancy grid map 620 contains aprobability of occupancy based on the accumulation of range finder dataresulting from the existence of an object 12 falling within that cell624. In some examples, each cell 624 has a fixed size, such as 5 cm by 5cm. The occupancy grid map 620 accumulates range data 515 having x and ycoordinates from the range finding sensor(s) 510, 510 a-d by receivingrange points 515 in cells 624 occupying the corresponding x, y values ofthe range points 515. For example, a first cell occupying x=0−5 cm andy=0−5 cm receives a point 515 having an x value of 3 cm and a y value of2 cm. Each cell 624 may have a classification. A cell 624 f having aprobability of occupancy less than 50%, for example, may be classifiedas unoccupied or free, whereas a cell 624 o having an occupancy greaterthan or equal to 50%, in this example may be classified as occupied.

Referring to FIGS. 6-8B, the SLAM controller 610 may use a particlefilter 630 (e.g., a Kalman filter or extended Kalman filter (EKF))having a particle model 645 made up of a set of particles 640 _(1-n)that represent hypotheses of a pose P of the robot 100 in itsenvironment. Each particle 640 _(n) builds its own associated map data.As a result, each particle 640 _(n) may have an associated particle map642, robot pose 644, and/or a particle weight 646. Each particle 640_(n) may represent a hypothesis, in terms of its associated map 642 androbot pose 644, of the location of the robot 100 with respect to itssurroundings. The SLAM controller 610 may score and attribute a particleweight 646 to each particle 640 _(n).

The pose P or location in space (i.e., x, y, and θ in Euclidean planarspace) of a mobile robot usually changes in time, so data aggregationmay involve referencing spatial measurements back to a common datum. Ingeneral, SLAM algorithms estimate a new pose P of the robot 100 based onthe commanded motion and subsequent distance measurements from the robot100 to the outside world (i.e., the scene 10). In order for the posedistribution (probabilistic estimate of the true location) to converge,the distance measurements should be relatively unique. For example, asingle distance measurement in an empty square room may provide someinformation about the robot's location; however, a determined pose Pbased on so much ambiguity and estimation may do more harm than good forlocalizing the robot 100. Given the same situation, but having 10different range measurements likely allows the robot 100 to determine ifit is near a wall or a corner and even which particular wall or cornerit is near.

The navigation system 600 may employ data aggregation, a technique usedto construct a single dataset from groups of data collected acrossdifferent sensor types (e.g., sonar, tactile, and/or proximity sensors)and/or at various times when acquired sensor data is sparse, which mayresult from sensors with low spatial resolution and/or slow updaterates. In some implementations, the SLAM controller 610, with dataaggregation enabled, aggregates sensor readings across sensors (e.g.,four separate IR range finders 510 a-d and a virtual range sensor basedon a wall-follower infrared proximity sensor 530/wall follower behavior)and across time. For example, as the robot 100 moves forward, the rangefinders 510 a-d capture range data (e.g., 30 points/meter). Unlike arotating scanner that may obtain about 360 points per 360 degreerevolution, the range finders 510 a-d may capture relatively fewerpoints. As a result, rather than updating the map 620 after eachsampling, the SLAM controller 610 updates the map 620 after a collectionof samplings (i.e., a collection of points).

The SLAM controller 610 may update the map 620 on a periodic basis, suchas after travelling a threshold distance (e.g., once every meter or halfmeter travelled by the robot 100), after a threshold period of time(e.g., once every 1-3 seconds), or after collecting a threshold numberof points (e.g., greater than 20 points). To aggregate across time theSLAM controller 610 stores sensor measurements as range and bearingpairs originating from the center of the robot 100, along with a bestestimate of the robot's pose P (x, y, and θ measurements of the robot'scenter to an arbitrarily oriented coordinate system established when theparticular run began).

The SLAM controller 610 may accumulate range data 515 in a data buffer612 (e.g., non-transitory memory) until it has acquired a predefinedamount. During the accumulation period, the position or pose P of therobot 100 becomes less and less well-known, since the robot 100continues to move, which adds uncertainty. Once the data buffer 612 isfull or reaches a threshold capacity, the SLAM controller 610 processesthe buffered range data 515 as a group and updates the map 620 andcorrects the robot's position or pose P. The SLAM controller 610 maybalance a cost in increased error when moving with a correction accuracygain that results from accumulating more range data 515. In someimplementations, the SLAM controller 610 processes the range data aftercollecting 100 points. This may correspond to approximately 4 feet orless of travel between updates, but could be more depending on theparticular geometry of a given room and the maximum detection range ofthe sensor. The proper balance for any robot is a function of thequality of the odometry sensor(s) and range sensor(s), and theenvironments that it's expected to operate in.

In some implementations, the SLAM controller 610 forces processing ofthe accumulated data if the robot 100 travels beyond a certain distanceor rotates through a certain number of degrees. The trigger forprocessing data may also be dynamically based on other measures of thesystem (e.g., a current confidence of the localization of the robot 100,a quality of the accumulated data, etc.). In some examples, the SLAMcontroller 610 keeps points that match the map 620 and drop any pointsthat do not. The SLAM controller 610 may execute a matching routine thatcomputes how well a given point matches the map (e.g., is within athreshold distance of a known point). If the range finders 510 a-d failto receive a reflection from an object, rather than assuming an objectis not present, the SLAM controller 610 may ignore the correspondingrange data (in lieu of other subsequently acquired data).

The navigation system 600 may include a localization quality estimator650 for determining a confidence in the localization of the robot 100.Rather than assuming that all range data received by the robot 100 willbe helpful in improving the robot's localization and the building of themap 620, which may be true when the available range data 515 is rich andgenerally accurate, the SLAM controller 610 may receive a confidencelevel of the robot localization from the localization quality estimator650 before making any such determination. When the collected range data515 is abundant, the robot 100 may be unlikely to become grosslydelocalized. However, when the collected range data 515 is sparse, hasvery limited range, or becomes highly variable when the measureddistance is greater than a threshold distance, such as about one meterfor infrared ranging sensors, there may be periods of time when thenavigation system 600 does not have a very high confidence in the pose Pof the robot 100. The SLAM controller 610 may use the localizationquality estimator 650 to assess the quality Q of the believed pose P ofthe robot 100.

In some implementations, the localization quality estimator 650 uses theparticle weights 646 to estimate if the robot 100 has become delocalizedby determining an average particle weight 646 for the particles 640 ofthe particle model 645 (or a subset thereof) to provide an estimate oflocalization quality Q. If the robot 100 is well-localized the averageweight 646 is high, otherwise it is low. The localization qualityestimator 650 may apply one or more filters to smooth out thedetermination. The localization quality estimator 650 may determine aninstantaneous localization of the robot 100 (i.e., the localizationquality according to the data available from the most recent rangemeasurements 515) by processing the instantaneous localization through afast low-pass filter 1010 and a slow low-pass filter 1020 and comparingthe ratio of two low-pass filters 1010, 1020. The localization qualityestimator 650 may then put the instantaneous localization resultsthrough a leaky integrator 1030, so as to estimate a stablelocalization. When the value of the integrator 1030 is above apredefined threshold the robot 100 is considered to be well localized.

The SLAM controller 610 may update particles 640 n in the particle model645 using a robot motion model 660. The robot motion model 660 modelsmovement, including corresponding measurement error of the robot 100.Using odometry and/or an inertial measurement unit (IMU) 520, the robotmotion model 660 measures motion (e.g., travel distance and/or travelpath) of the robot 100. Due to drive wheel slip, encoder tolerances,etc., the measured odometry may include an error term. For example, ifthe robot 100 rotates 90° and translates one meter, the measuredodometry may be off by +/−10° of rotation and +/−5 cm of translation.Moreover, the IMU 520 may have a different error, depending on it methodof measurement (e.g., gyro). The robot motion model 660 may be aGaussian error model centered on a travel vector derived from a travelvector derived from odometry and/or the IMU 520, where some portion ofthe Gaussian curve represents noise.

The SLAM controller 610 may receive an input of range data 515estimating locations of features 14 of the environment 10 relative tothe robot 100 (e.g., relative to the center C of the robot 100). TheSLAM controller 610 receives distance estimates of obstacles 12 at agiven height off of the floor 5 as dictated by the mounting position andorientation of the sensor (e.g., range finders 510).

From the individual pose 644 (position hypotheses) of the particles 640_(n), the particle filter 630 may select the hypothesis of the particle640 _(n) having the highest weight 646 as a best pose 644 or positionhypothesis of the robot 100 at a current time. The robot controller 150can use the pose 644 or position hypothesis of the selected particle 640_(n) and its associated particle map 642 to determine a way point forissuing a drive command to the drive system 200. In other words, therobot controller 150 uses the particle map 642 of the selected particle640 _(n) to navigate at that given moment in time. For example, if therobot 100 has a drive goal to drive from a first room to a second room,the navigation system 600 may take a snapshot of a selected particle 640_(n) (i.e., position hypothesis and/or maps) at a time of drive commandissuance, and perform localization temporarily of that particle'shypothesis and that particle's map 642.

The SLAM controller 610 may update the particles 640 _(n) in aprobabilistic manner using the robot motion model 660. The robot motionmodel 660 provides a probability distribution of new positions to whichthe robot 100 may have moved (e.g., based on odometry and/or an IMU520). For example, if the SLAM controller 610 estimates that the robot100 moved forward one meter, but with some error in that movement,rather than updating all of the particles 640 _(n) by moving them allforward one meter, for each particle 640 _(n), the SLAM controller 610generates a sample based on a position distribution of the robot motionmodel 660 centered on one meter of forward movement with a thresholdvariance (e.g., 10%). A first particle 640 ₁ may receive a sample of 0.9meters of forward movement. A second particle 640 ₂ may receive a sampleof 1.1 meters of forward movement. A third particle 640 ₃ may receive asample of 1.05 meters of forward movement, and so on, such that eachparticle 640 _(n) receives its own sample based on the positiondistribution. The SLAM controller 610 can update each particle 640 _(n)for motion based on its received sample.

The SLAM controller 610 may build the map 620 only when the robot 100 iswell localized (i.e., has a good confidence of its location). If therobot 100 is not well localized, the navigation system 600 may cause thecontroller 150 to issue a drive command to maneuver the robot 100 to aknown object, such as in contact with a wall or corner, to localize therobot 100 with respect to that known object. Moreover, to improvelocalization, the navigation system 600 may execute path planning for apath within a field of view 512 of the range finders 510 a-d or othersensors of the sensor system 500. If the robot 100 becomes lost, it mayexecute a behavior 300 that moves the robot 100 in a manner (e.g.,spiral movement) that probabilistically will find an object 12, such asa wall. The robot 100 may execute the wall-following behavior 300 b tonot only follow the wall to re-localize, but also use the wall-followingas a virtual proximity sensor when the robot 100 follows the wall at afixed distance or senses contact with the wall using the bump sensor(s)530.

When the SLAM controller 610 is confident that the robot pose P iswell-known, it may use the range data 515 of the range finders 510 a-dto add occupancy probabilities to the occupancy grid map/model 620.However, if the SLAM controller 610 is not confident in the robot'slocalization, it may skip the step of modifying/updating the occupancygrid map/model 620. All other parts of the SLAM process may proceed asusual. This technique causes the map 620 to become frozen in the lastknown-good state and to remain that way until the robot 100 is able toregain a high-quality estimate of its position (pose P), at which timethe SLAM controller 610 may resume adding occupancy data to the map 620.

In short, this technique helps prevent the system from destroying aperfectly good map 620 due to what would otherwise be a temporary lossof localization. If the map 620 becomes significantly corrupted it maybe very difficult, if not impossible, to localize successfully.

The SLAM controller 610 may execute selective resampling. Resampling mayoccur after the SLAM controller 610 processes all of the motion andrange data and computes a weight 646 for each particle 640 n in theparticle model 645. The particle weight 646 of a particle 640 n is basedon a degree of agreement between its corresponding particle map 642 andthe range data collected from the range finding sensor(s) 510 a-d.Resampling is an evolutionary process in which strongly weightedparticles 640 n are more likely to be selected into a new particlepopulation or particle model 645 than weakly weighted particles 640 n.In some examples, resampling occurs on each cycle of the SLAM algorithm.

When executing with sparse range data, the SLAM controller 610 may findit difficult to distinguish strong particles 640 n from weak particles640 n in some cycles of the SLAM algorithm. At such times, the shape ofthe particle weight distribution may be relatively flat and resamplingmay be unhelpful. In some instances, resampling may be harmful in such asituation, because weak particles 640 n have a greater chance ofreplicating than they should, and strong particles 640 n have a lesserchance of replicating than they ought to. The solution to this problemis to resample only when there is adequate difference in the weightingof the particles 640 n that make up the particle model 645. When thereis not enough differentiation, the resampling step is skipped and therest of the algorithm continues as usual.

If the SLAM controller 610 encounters a long period of time withoutresampling, as may occur if the model were to become significantlydamaged, the SLAM controller 610 may replace large portions of theparticle distribution with random particles 640 n (or particles placedwith intelligent bias based on some characteristic of the model or oncertain sensor data).

FIGS. 9-12 illustrate an exemplary arrangement of operations for amethod of simultaneous localization and mapping (SLAM). FIG. 9 providesan exemplary arrangement 900 of operations for a main routine of themethod. The method includes initializing a robot pose P, such as x=0,y=0, and θ=0°, and initializing a particle model 645 having n particles640. Associated with each particle 640 n is a map 642, a pose 644, and aweight 646 (i.e., confidence rating). The method further includesestimating a change in the robot pose ΔPose (i.e., as dx, dy, dθ), forexample, using wheel odometry 205 and/or gyroscopic data 525 (e.g., fromthe inertial measurement unit 520), acquiring range data 515 (e.g., formthe range finding sensor(s) 510 a-d), and optionally acquiring bumperdata 535 (e.g., from the bumper sensor 530). The method includessynchronizing range data 515 and bumper data 535 with change(s) in therobot pose (i.e., ΔPose data), estimating a localization quality Q(e.g., using the localization quality estimator 650), and updating eachparticle 640 n in the particle model 645. The method includes computingthe strength or weight 646 of a particle bunch, for example, using amean, fast low-pass filter, and/or a slow low-pass filter. If the meanstrength 646 of the particle bunch is above a threshold strength 646,the method includes re-sampling the particle distribution 645, setting apose belief P to that of the strongest particle pose 644, and returningto the step of synchronizing newly acquired range data 515 and bumperdata 535 with newly acquired ΔPose data (e.g., odometry 205). If themean strength 646 of the particle bunch 645 is below the thresholdparticle strength 646, the method includes returning to the step ofsynchronizing newly acquired range data 515 and bumper data 535 withnewly acquired ΔPose data (e.g., odometry 205). As a result, the methodmay include resampling only when the SLAM Controller 610 candifferentiate between strong and weak particles 640 n.

FIG. 10A provides an exemplary arrangement 1000 of operations for amethod of determining a robot localization quality Q. The methodincludes computing an instantaneous localization value, which may be thevalue of the particle strength a passed through a fast low-pass filterand referenced to the strength as passed through a slower low-passfilter. If the instantaneous localization is above a threshold quantity,the method includes increasing a localization score (if the localizationscore is below a maximum value). Otherwise, the method includesdecreasing a localization score (if the localization score is above aminimum value). If the localization score is above a threshold score,the robot 100 is considered localized.

FIG. 10B provides an exemplary arrangement 1001 of operations for amethod of robot re-localization. If the robot 100 determines it isdelocalized (e.g., based on the robot localization quality Q), ratherthan degrading its map 620, the robot 100 may decide to use an old map620 that the robot 100 knows is good and/or repopulate the particlemodel 645. In other words, rather than using bad information to updatethe map 620 (e.g., contributing to map drift) when the robot 100 becomesdelocalized, the robot 100 may stop building the map 620 until itbecomes re-localized again. In some implementations, the SLAM controller610 may replace large portions of the particle distribution with clonesof “good” particles 640 and/or random particles 640 n. For example, theSLAM controller 610 may access a last know “good” particle 640 andcreate a new particle bunch from that “good” particle 640. The SLAMcontroller 610 may maintain a time-series buffer (e.g., record innon-transitory memory) of the best maps 620 and poses P of the robot 100over a period of time. The SLAM controller 610 may copy or partiallycopy one or more “good” particles 640 corresponding to the buffered bestmaps 620 and poses P of the robot 100 to repopulate the particle model645. Moreover, the SLAM controller 610 may add a number of randomlygenerated particles 640 and/or kill off any determined “bad” particles640 as well. The robot 100 may switch to a localization-only mode andmaneuver itself about until it becomes confidently localized. Oncelocalized again, the robot 100 may switch back to a SLAM-mode.

In some implementations, when the robot 100 determines it isdelocalized, the robot controller 150 executes the wall-followingbehavior 300 b to follow a wall for re-localization. The robot 100 mayuse the wall-following as a virtual proximity sensor when the robot 100follows the wall at a fixed distance or senses contact with the wallusing the bump sensor(s) 530. Moreover, the SLAM controller 610 may addnew particles 640 to the particle model 645 based on the virtualproximity sensor data, allowing the robot to not only re-localize off ofthe wall, but also add reliable particles 640 to the particle model 645.

FIG. 11 provides an exemplary arrangement 1100 of operations for amethod of updating a particle 640 n in the particle model 645. Themethod includes applying the robot motion model 660 for any change inrobot pose (ΔPose=dx, dy, dθ). The robot motion model 660 may berepresented as:x=x+dx+GN*C1*dx+GN*C2*dy  (1)y=y+dy+GN*C2*dy+GN*C4*dx  (2)θ=θ+dθ+GN*C5*dθ+GN*C6*(dx+dy)  (3)

where GN=Gaussian noise.

The method further includes adding valid range data 515 to the databuffer 612, for example, using a current robot pose P as an origin. Ifthe range data clustering is enabled, the method includes checking if adata buffer threshold has been met. If the data buffer threshold has notbeen met, then the method returns (e.g., to the main SLAM method). Ifthe range data clustering is disabled or the data buffer threshold hasbeen met, the method includes executing a ray trace for each range databearing. An exemplary input for the ray trace may include a robotbearing and/or a map 620, 642, and an exemplary output may include apredicted range for a given bearing. The method further includesapplying a range sensor model 670 (FIG. 7). An exemplary input for therange sensor model 670 may include a measured range and the predictedrange, and an exemplary output may include a probability of occupancy ofeach cell along the ray trace. The method includes determining if alocalization of the robot 100 is above a threshold quality Q based on areceived robot state. If the robot 100 is well localized, the methodincludes updating a particle map 642 with the output of the range sensormodel 670, reducing the occupancy probability of cells 624 of the map620 underneath the robot 100, and computing a new particlestrength/weight 646. If the robot 100 is not well localized, the methodproceeds to computing the new particle strength/weight 646. The particlestrength/weight 646 can be based on a number of measured ranges thatmatch the expected ranges, based on the particle map 642. Afterwards,the method returns (e.g., to the main SLAM method).

FIG. 12 provides an exemplary arrangement 1200 of operations for amethod of applying the range sensor model 670. The method includesdetermining if a predicted range is available. This may entaildetermining if a map value exists at a ranged location. If a predictedrange is available, the method includes determining a range error, whereRange Error=abs(Measured Range−Predicted Range)  (4)and determining a first probability of occupancy assuming an expectedtarget is detected by the robot 100. This may entail assigning one offive range values (e.g., close, somewhat close, medium, somewhat far,and far) based on a magnitude of range error. The method furtherincludes determining a second probability of occupancy assuming anexpectedly close target is detected by the robot 100. This may entailassigning one of three range values (e.g., close, medium, and far) basedon a range difference.Range Difference=abs(Measured Range−Actual Range)−abs(PredictedRange−Actual Range)  (5)

The assigned range value can be based on how much closer the measuredrange was to the target object than the predicted range. The methodfurther includes determining a third probability of occupancy assumingthat a long range measurement results from a failure to detect a closeobject. This may entail assigning a predetermined value.

If a predicted range is not available, the method includes determining afourth probability of occupancy an expected target object was detected.This may entail assigning one of three range values (e.g., close,medium, and far) based on a magnitude of the measured range.

The method includes determining a fifth probability of occupancy of afinal target object, which can be a weighted average of each of thedetermined probabilities of occupancy (e.g., of the first, second andthird probabilities of occupancy or of the fourth probability ofoccupancy). The method also includes determining a sixth probability ofoccupancy for cells between the robot and the measured range to thetarget object. This may entail assigning values based on how close cellsare to the robot. The sixth probability of occupancy (also referred toas the cell probability of occupancy) is returned.

FIG. 13 provides an exemplary arrangement 1300 of operations for amethod of simultaneous localization and mapping executed on the SLAMcontroller 610 of the robot 100. The method includes initializing 1302the robot pose P and initializing 1304 the particle model 645 of theparticle filter 630 executing on the SLAM controller 610. The particlemodel 645 includes particles 640 n, each having an associated map 642,robot pose 644, and weight 646. The method also includes receiving 1306sparse sensor data 205, 515 (e.g., odometry 205, range data 515,inertial data 525, and/or bump data 535) from the sensor system 500 ofthe robot 100, synchronizing 1308 the received sensor data 205, 515,525, 535 with a change in robot pose ΔP, accumulating 1310 thesynchronized sensor data 205, 515, 525, 535 over time in non-transitorymemory 612 (buffer), and determining 1312 a localization quality Q ofthe robot 100. When the accumulated sensor data 205, 515, 525, 535exceeds a threshold accumulation and the robot localization quality Q isgreater than a threshold localization quality, the method includesupdating 1314 particles 640 n of the particle model 645 with accumulatedsynchronized sensor data 205, 515, 525, 535. The method includesdetermining 1316 a weight 646 for each updated particle 640 n of theparticle model 645 and setting 1318 a robot pose belief P to the robotpose 644 of the particle 640 n having the highest weight 646 when a meanweight 646 of the particles 640 is greater than a threshold particleweight 646.

In some implementations, the method includes sampling the particle model645 for a particle population 640 n and resampling the particle model645 for the particle population when the robot localization quality Q isgreater than the threshold localization quality Q. The method mayinclude replacing a portion of the particle population 640 n withrandomly selected particles 640 n of the particle model 645 whenresampling fails to occur within a threshold sampling time period.Additionally or alternatively, the method may include replacing aportion of the particle population 640 n with particles 640 n selectedfrom the particle model 645 based on the accumulated synchronized sensordata 205, 515, 525, 535 when resampling fails to occur within thethreshold sampling time period.

The method may include determining the change in robot pose ΔP usingodometry 205 received from a drive system 200 of the robot 100 andinertial measurements 525 from an inertial measurement unit 520 of therobot 100. In some examples, determining the robot localization qualityQ includes computing an instantaneous localization (x, y, θ), receivingthe instantaneous localization (x, y, θ) through a fast low-pass filter1010, receiving the instantaneous localization (x, y, θ) through a slowlow-pass filter 1020, and receiving the instantaneous localization (x,y, θ) through a leaky integrator 1030. The method may include comparinga ratio of results of the two low-pass filters 1010, 1020 integrated bythe integrator 1030. When a result of the integrator 1030 is above athreshold value, the method may include determining the robot 100 aswell localized (e.g., by assigning a localization quality Qaccordingly).

The sensor data 205, 515, 525, 535 may include range data 515 (e.g.,from one or more infrared range finding sensors) and the synchronizedsensor data 205, 515, 525, 535 include range measurement and bearingpairs. The bearings may be determined from the known mount locations ofthe sensors 510 a-d.

In some implementations, the method includes updating the map 642, thepose 644, and the weight 646 of each particle 640 n. Updating theparticle pose 644 may include applying a robot motion model 660. Therobot motion model 660 models movement of the robot 100 based onodometry 205 and inertial measurements 525. Updating the particle map642 may include executing a ray trace of the accumulated synchronizedsensor data 205, 515, 525, 535 and applying a range sensor model 670 tothe accumulated synchronized sensor data 205, 515, 525, 535. Updatingthe particle weight 646 may be based on a number of measured ranges 515of the accumulated synchronized sensor data 205, 515, 525, 535 matchingpredicted ranges 515.

Executing the ray trace may include, for each range measurement andbearing pair, receiving the bearing 205 and an existing particle map642, and computing a predicted range 515. The method may further includereceiving a measured range 515, receiving the predicted range 515, andcomputing a probability of occupancy of an object 12 in each cell 622 ofthe particle map 642 along the ray trace. In some examples, applying therange sensor model 670 includes only updating particle map cells 624corresponding to the synchronized sensor data 205, 515, 525, 535.

In some implementations, applying the range sensor model 670 includescomputing a range error as an absolute difference between the measuredrange 515 and the predicted range 515 and computing the occupancyprobability within each cell 624 of the particle map 642 along the raytrace based on the range error. For each particle map cell 624 along theray trace, the method may include computing a first occupancyprobability assuming detection of an expected object 12, computing asecond occupancy probability assuming detection of an expectedly closeobject 12, computing a third occupancy probability assuming failure todetect of an expected close object 12 within a threshold distance of therobot 100, and computing a weighted average of the first, second, andthird occupancy probabilities.

Various implementations of the systems and techniques described here canbe realized in digital electronic circuitry, integrated circuitry,specially designed ASICs (application specific integrated circuits),computer hardware, firmware, software, and/or combinations thereof.These various implementations can include implementation in one or morecomputer programs that are executable and/or interpretable on aprogrammable system including at least one programmable processor, whichmay be special or general purpose, coupled to receive data andinstructions from, and to transmit data and instructions to, a storagesystem, at least one input device, and at least one output device.

These computer programs (also known as programs, software, softwareapplications or code) include machine instructions for a programmableprocessor, and can be implemented in a high-level procedural and/orobject-oriented programming language, and/or in assembly/machinelanguage. As used herein, the terms “machine-readable medium” and“computer-readable medium” refer to any computer program product,apparatus and/or device (e.g., magnetic discs, optical disks, memory,Programmable Logic Devices (PLDs)) used to provide machine instructionsand/or data to a programmable processor, including a machine-readablemedium that receives machine instructions as a machine-readable signal.The term “machine-readable signal” refers to any signal used to providemachine instructions and/or data to a programmable processor.

Implementations of the subject matter and the functional operationsdescribed in this specification can be implemented in digital electroniccircuitry, or in computer software, firmware, or hardware, including thestructures disclosed in this specification and their structuralequivalents, or in combinations of one or more of them. Moreover,subject matter described in this specification can be implemented as oneor more computer program products, i.e., one or more modules of computerprogram instructions encoded on a computer readable medium for executionby, or to control the operation of, data processing apparatus. Thecomputer readable medium can be a machine-readable storage device, amachine-readable storage substrate, a memory device, a composition ofmatter effecting a machine-readable propagated signal, or a combinationof one or more of them. The terms “data processing apparatus”,“computing device” and “computing processor” encompass all apparatus,devices, and machines for processing data, including by way of example aprogrammable processor, a computer, or multiple processors or computers.The apparatus can include, in addition to hardware, code that creates anexecution environment for the computer program in question, e.g., codethat constitutes processor firmware, a protocol stack, a databasemanagement system, an operating system, or a combination of one or moreof them. A propagated signal is an artificially generated signal, e.g.,a machine-generated electrical, optical, or electromagnetic signal, thatis generated to encode information for transmission to suitable receiverapparatus.

A computer program (also known as an application, program, software,software application, script, or code) can be written in any form ofprogramming language, including compiled or interpreted languages, andit can be deployed in any form, including as a stand alone program or asa module, component, subroutine, or other unit suitable for use in acomputing environment. A computer program does not necessarilycorrespond to a file in a file system. A program can be stored in aportion of a file that holds other programs or data (e.g., one or morescripts stored in a markup language document), in a single filededicated to the program in question, or in multiple coordinated files(e.g., files that store one or more modules, sub programs, or portionsof code). A computer program can be deployed to be executed on onecomputer or on multiple computers that are located at one site ordistributed across multiple sites and interconnected by a communicationnetwork.

The processes and logic flows described in this specification can beperformed by one or more programmable processors executing one or morecomputer programs to perform functions by operating on input data andgenerating output. The processes and logic flows can also be performedby, and apparatus can also be implemented as, special purpose logiccircuitry, e.g., an FPGA (field programmable gate array) or an ASIC(application specific integrated circuit).

Processors suitable for the execution of a computer program include, byway of example, both general and special purpose microprocessors, andany one or more processors of any kind of digital computer. Generally, aprocessor will receive instructions and data from a read only memory ora random access memory or both. The essential elements of a computer area processor for performing instructions and one or more memory devicesfor storing instructions and data. Generally, a computer will alsoinclude, or be operatively coupled to receive data from or transfer datato, or both, one or more mass storage devices for storing data, e.g.,magnetic, magneto optical disks, or optical disks. However, a computerneed not have such devices. Moreover, a computer can be embedded inanother device, e.g., a mobile telephone, a personal digital assistant(PDA), a mobile audio player, a Global Positioning System (GPS)receiver, to name just a few. Computer readable media suitable forstoring computer program instructions and data include all forms of nonvolatile memory, media and memory devices, including by way of examplesemiconductor memory devices, e.g., EPROM, EEPROM, and flash memorydevices; magnetic disks, e.g., internal hard disks or removable disks;magneto optical disks; and CD ROM and DVD-ROM disks. The processor andthe memory can be supplemented by, or incorporated in, special purposelogic circuitry.

To provide for interaction with a user, one or more aspects of thedisclosure can be implemented on a computer having a display device,e.g., a CRT (cathode ray tube), LCD (liquid crystal display) monitor, ortouch screen for displaying information to the user and optionally akeyboard and a pointing device, e.g., a mouse or a trackball, by whichthe user can provide input to the computer. Other kinds of devices canbe used to provide interaction with a user as well; for example,feedback provided to the user can be any form of sensory feedback, e.g.,visual feedback, auditory feedback, or tactile feedback; and input fromthe user can be received in any form, including acoustic, speech, ortactile input. In addition, a computer can interact with a user bysending documents to and receiving documents from a device that is usedby the user; for example, by sending web pages to a web browser on auser's client device in response to requests received from the webbrowser.

One or more aspects of the disclosure can be implemented in a computingsystem that includes a backend component, e.g., as a data server, orthat includes a middleware component, e.g., an application server, orthat includes a frontend component, e.g., a client computer having agraphical user interface or a Web browser through which a user caninteract with an implementation of the subject matter described in thisspecification, or any combination of one or more such backend,middleware, or frontend components. The components of the system can beinterconnected by any form or medium of digital data communication,e.g., a communication network. Examples of communication networksinclude a local area network (“LAN”) and a wide area network (“WAN”), aninter-network (e.g., the Internet), and peer-to-peer networks (e.g., adhoc peer-to-peer networks).

The computing system can include clients and servers. A client andserver are generally remote from each other and typically interactthrough a communication network. The relationship of client and serverarises by virtue of computer programs running on the respectivecomputers and having a client-server relationship to each other. In someimplementations, a server transmits data (e.g., an HTML page) to aclient device (e.g., for purposes of displaying data to and receivinguser input from a user interacting with the client device). Datagenerated at the client device (e.g., a result of the user interaction)can be received from the client device at the server.

While this specification contains many specifics, these should not beconstrued as limitations on the scope of the disclosure or of what maybe claimed, but rather as descriptions of features specific toparticular implementations of the disclosure. Certain features that aredescribed in this specification in the context of separateimplementations can also be implemented in combination in a singleimplementation. Conversely, various features that are described in thecontext of a single implementation can also be implemented in multipleimplementations separately or in any suitable sub-combination. Moreover,although features may be described above as acting in certaincombinations and even initially claimed as such, one or more featuresfrom a claimed combination can in some cases be excised from thecombination, and the claimed combination may be directed to asub-combination or variation of a sub-combination.

Similarly, while operations are depicted in the drawings in a particularorder, this should not be understood as requiring that such operationsbe performed in the particular order shown or in sequential order, orthat all illustrated operations be performed, to achieve desirableresults. In certain circumstances, multi-tasking and parallel processingmay be advantageous. Moreover, the separation of various systemcomponents in the embodiments described above should not be understoodas requiring such separation in all embodiments, and it should beunderstood that the described program components and systems cangenerally be integrated together in a single software product orpackaged into multiple software products.

A number of implementations have been described. Nevertheless, it willbe understood that various modifications may be made without departingfrom the spirit and scope of the disclosure. Accordingly, otherimplementations are within the scope of the following claims. Forexample, the actions recited in the claims can be performed in adifferent order and still achieve desirable results.

What is claimed is:
 1. A method comprising: receiving, at a computingdevice, sensor data from an autonomous mobile robot operating in a workenvironment; updating, using the computing device, an occupancy map ofthe work environment with location occupancy probabilities based on thereceived sensor data; determining, using the computing device, alocalization quality of the autonomous mobile robot; and when thelocalization quality does not satisfy a threshold localization quality,executing, using the computing device, a wall-following behavior thatcauses the autonomous mobile robot to maneuver toward a wall in the workenvironment and drive adjacent to the wall until the localizationquality satisfies the threshold localization quality.
 2. The method ofclaim 1, wherein the wall-following behavior uses proximity sensor datafrom a proximity sensor of the autonomous mobile robot to cause theautonomous mobile robot to maintain a threshold distance from the wallwhile driving adjacent the wall.
 3. The method of claim 1, furthercomprising, when the localization quality does not satisfy the thresholdlocalization quality, refraining from updating the occupancy map.
 4. Themethod of claim 1, further comprising, when the localization qualitydoes not satisfy the threshold localization quality: identifying alocation of the wall using the occupancy map; and issuing a drivecommand from the computing device to a drive system of the autonomousmobile robot to maneuver the autonomous mobile robot toward theidentified location of the wall.
 5. The method of claim 4, furthercomprising: after updating the occupancy map, storing the updatedoccupancy map in memory storage hardware in communication with thecomputing device; and when the localization quality does not satisfy thethreshold localization quality: retrieving a previously stored occupancymap from the memory storage hardware; and identifying the location ofthe wall using the previously stored occupancy map.
 6. The method ofclaim 1, further comprising: synchronizing the received sensor data witha change in robot pose; accumulating the synchronized sensor data overtime in memory storage hardware in communication with the computingdevice; and when the received sensor data exceeds a thresholdaccumulation and the localization quality satisfies the thresholdlocalization quality, updating particles of a particle model of aparticle filter with the accumulated synchronized sensor data, eachparticle having an associated particle occupancy map, robot pose, andweight.
 7. The method of claim 6, wherein the sensor data comprisesrange data from at least one range finding sensor of the autonomousmobile robot and the synchronized sensor data comprises rangemeasurement and bearing pairs.
 8. The method of claim 7, wherein thethreshold accumulation of the sensor data comprises at least 20 rangemeasurements from the at least one range finding sensor.
 9. The methodof claim 6, wherein determining the robot localization quality comprisesdetermining an average weight of the particles of the particle model.10. The method of claim 6, further comprising: determining the weightfor each updated particle of the particle model based on whether thereceived sensor data corroborates the corresponding particle occupancymap; and setting a robot pose belief to the robot pose of the particlehaving a highest weight when a mean weight of the particles is greaterthan a threshold particle weight.
 11. The method of claim 6, furthercomprising, when the localization quality does not satisfy the thresholdlocalization quality, adding new particles to the particle model basedon the sensor data received while executing the wall-following behavior.12. The method of claim 6, further comprising: sampling the particlemodel for a particle population; and determining, using the computingdevice, an instantaneous localization of the autonomous mobile robotbased on at least one particle of the particle population.
 13. Themethod of claim 12, further comprising resampling the particle model forthe particle population when the localization quality satisfies thethreshold localization quality.
 14. The method of claim 12, furthercomprising, when the localization quality does not satisfy the thresholdlocalization quality, replacing a portion of the particle populationwith particles selected from the particle model based on the accumulatedsynchronized sensor data.
 15. The method of claim 1, wherein determiningthe robot localization quality comprises: computing an instantaneouslocalization based on the sensor data; receiving the instantaneouslocalization through a fast low-pass filter; receiving the instantaneouslocalization through a slow low-pass filter; and receiving theinstantaneous localization through a leaky integrator; wherein thelocalization quality does not satisfy the threshold localization qualitywhen an integrator value of the leaky integrator fails to satisfy anintegrator threshold.
 16. A method comprising: receiving, at a computingdevice, sensor data from an autonomous mobile robot operating in a workenvironment; updating, using the computing device, an occupancy map ofthe work environment with location occupancy probabilities based on thereceived sensor data; determining, using the computing device, alocalization quality of the autonomous mobile robot; and when thelocalization quality does not satisfy a threshold localization quality:identifying, using the computing device, a mapped object on theoccupancy map; and issuing a drive command from the computing device toa drive system of the autonomous mobile robot to maneuver the autonomousmobile robot toward the mapped object in the work environment tore-localize the autonomous mobile robot based on the sensor data. 17.The method of claim 16, further comprising: comparing, using thecomputing device, the sensor data relative to the mapped object on theoccupancy map; and determining, using the computing device, a posebelief of the autonomous mobile robot based on the comparison.
 18. Themethod of claim 16, further comprising, when the localization qualitydoes not satisfy the threshold localization quality, refraining fromupdating the occupancy map.
 19. The method of claim 16, furthercomprising: after updating the occupancy map, storing the updatedoccupancy map in memory storage hardware in communication with thecomputing device; and when the localization quality does not satisfy thethreshold localization quality: retrieving a previously stored occupancymap from the memory storage hardware; and issuing the drive commandbased on the previously stored occupancy map.
 20. The method of claim16, further comprising: synchronizing the received sensor data with achange in robot pose; accumulating the synchronized sensor data overtime in memory storage hardware in communication with the computingdevice; and when the received sensor data exceeds a thresholdaccumulation and the localization quality satisfies the thresholdlocalization quality, updating particles of a particle model of aparticle filter with the accumulated synchronized sensor data, eachparticle having an associated particle occupancy map, robot pose, andweight.
 21. The method of claim 20, wherein the sensor data comprisesrange data from at least one range finding sensor of the autonomousmobile robot and the synchronized sensor data comprises rangemeasurement and bearing pairs.
 22. The method of claim 21, wherein thethreshold accumulation of the sensor data comprises at least 20 rangemeasurements from the at least one range finding sensor.
 23. The methodof claim 20, wherein determining the robot localization qualitycomprises determining an average weight of the particles of the particlemodel.
 24. The method of claim 20, further comprising: determining theweight for each updated particle of the particle model based on whetherthe received sensor data corroborates the corresponding particleoccupancy map; and setting a robot pose belief to the robot pose of theparticle having a highest weight when a mean weight of the particles isgreater than a threshold particle weight.
 25. The method of claim 20,further comprising, when the localization quality does not satisfy thethreshold localization quality, adding new particles to the particlemodel based on the sensor data received while the autonomous mobilerobot maneuvers toward the mapped object in the work environment. 26.The method of claim 20, further comprising: sampling the particle modelfor a particle population; and determining, using the computing device,an instantaneous localization of the autonomous mobile robot based on atleast one particle of the particle population.
 27. The method of claim26, further comprising resampling the particle model for the particlepopulation when the localization quality satisfies the thresholdlocalization quality.
 28. The method of claim 26, further comprising,when the localization quality does not satisfy the thresholdlocalization quality, replacing a portion of the particle populationwith particles selected from the particle model based on the accumulatedsynchronized sensor data.
 29. The method of claim 16, whereindetermining the robot localization quality comprises: computing aninstantaneous localization based on the sensor data; receiving theinstantaneous localization through a fast low-pass filter; receiving theinstantaneous localization through a slow low-pass filter; and receivingthe instantaneous localization through a leaky integrator; wherein thelocalization quality does not satisfy the threshold localization qualitywhen an integrator value of the leaky integrator fails to satisfy anintegrator threshold.